//
// Created by daybeha on 2022/3/8.
//

//subscriiber ID20(0x14)  data package

#include <ros/ros.h>
#include <iostream>
#include <unistd.h>
#include <serial_imu/NAV100_0x14.h>
#include "ch_serial.h"


#define RAD_TO_DEG   180/M_PI


void imu_0x14_callback(const serial_imu::NAV100_0x14 imu_0x14_msg);

int main(int argc,char **argv)
{
    ros::init(argc,argv,"sub_0x14");

    ros::NodeHandle n;

    ros::Subscriber imu_0x14_sub = n.subscribe("/nav100_0x14_package", 20, imu_0x14_callback);

    ros::spin();
}




void imu_0x14_callback(const serial_imu::NAV100_0x14 imu_0x14_msg)
{
    printf("\033c");

    printf("     Devie ID:%6d\n", imu_0x14_msg.imu_data.id);

    printf("    Run times: %d days  %d:%d:%d:%d\n", imu_0x14_msg.imu_data.time / 86400000, imu_0x14_msg.imu_data.time / 3600000 % 24, imu_0x14_msg.imu_data.time / 60000 % 60, imu_0x14_msg.imu_data.time / 1000 % 60, imu_0x14_msg.imu_data.time % 1000);

    printf("   Frame Rate:  %4dHz\r\n", imu_0x14_msg.imu_data.frame_rate);

    printf("   Gravity(g):  %8.3f\r\n", imu_0x14_msg.imu_data.gravity);

    printf("          GPS:%8.3f %8.3f %8.3f\r\n",imu_0x14_msg.imu_data.Latitude, imu_0x14_msg.imu_data.Longitude, imu_0x14_msg.imu_data.Height);
    printf("   GPS dev(m):%8.3f %8.3f %8.3f\r\n",imu_0x14_msg.imu_data.Latitude_dev, imu_0x14_msg.imu_data.Longitude_dev, imu_0x14_msg.imu_data.Height_dev);

    printf("Velocity(m/s):%8.3f %8.3f %8.3f\r\n",imu_0x14_msg.imu_data.v_north, imu_0x14_msg.imu_data.v_east, imu_0x14_msg.imu_data.v_down);

    printf("   Acc(m/s^2):%8.3f %8.3f %8.3f\r\n", imu_0x14_msg.imu_data.acc_x, imu_0x14_msg.imu_data.acc_y, imu_0x14_msg.imu_data.acc_z);
    printf("   Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x14_msg.imu_data.eul_r*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_p*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_y*RAD_TO_DEG);
    printf("   Gyr(rad/s):%8.2f %8.2f %8.2f\r\n", imu_0x14_msg.imu_data.gyr_x, imu_0x14_msg.imu_data.gyr_y, imu_0x14_msg.imu_data.gyr_z);
    printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu_0x14_msg.imu_data.quat_w, imu_0x14_msg.imu_data.quat_x, imu_0x14_msg.imu_data.quat_y, imu_0x14_msg.imu_data.quat_z);
}
